#include "fadapter.h"

#include <uavtmi/uavtmi.pb.h>

FAdapter* FAdapter::m_fadapter = 0;
FAdapter::_flight_telemetry FAdapter::ftm = {0,0,0,0,0,0,0,0,0,0};
fc_instance_t FAdapter::fclient;
QString FAdapter::command_responce;


int ftm_new ( ftm_data_storage_t *d ) {
    err ( __func__, "" );
    return FAdapter::instance()->handler_ftm_new(d);
}

int ftm_reset ( ftm_data_storage_t *d ) {
    err ( __func__, "" );
    return FAdapter::instance()->handler_ftm_reset(d);
}

int ftm_msg ( ftm_data_storage_t *d, char *msg ) {
    err ( __func__, "" );
    return FAdapter::instance()->handler_ftm_msg(d, msg);
}

int ftm_abt_req ( ftm_data_storage_t *d ) {
    err ( __func__, "" );
    return FAdapter::instance()->handler_ftm_abt_req(d);
}

int ftm_err ( ftm_data_storage_t *d, char *msg ) {
    err ( __func__, "" );
    return FAdapter::instance()->handler_ftm_err(d, msg);
}

int ftm_transmitted ( ftm_data_storage_t *d ) {
    err ( __func__, "" );
    return FAdapter::instance()->handler_ftm_transmitted(d);
}

int ftm_passed ( ftm_data_storage_t *d ) {
    err ( __func__, "" );
    return FAdapter::instance()->handler_ftm_passed(d);
}

int ftm_done ( ftm_data_storage_t *d, fc_report_inf_type_t type, char *msg ) {
    err ( __func__, "" );
    return FAdapter::instance()->handler_ftm_done(d, type, msg);
}


/// !!11

int mission_sync_creation_parse ( void *ud, char *cs, uint32_t uid, fc_command_type_t type, char *arg ) {
        err ( __func__, "%s", arg );
        return EOK;
}

int mission_sync_reset ( void *ud ) {
        err ( __func__, "" );
        return EOK;
}

int mission_sync_msg ( void *ud, uint32_t uid, char *msg ) {

    FAdapter::instance()->handler_mission_sync_msg(msg);

        err ( __func__, "0x%08X %s", uid, msg );
        return EOK;
}

int mission_sync_abt ( void *ud, uint32_t uid ) {
        err ( __func__, "0x%08X", uid );
        return EOK;
}

int mission_sync_err ( void *ud, uint32_t uid, char *msg ) {
        err ( __func__, "0x%08X %s", uid, msg );
        return EOK;
}

int mission_sync_inf ( void *ud, uint32_t uid, fc_report_inf_type_t type, char *msg ) {
        err ( __func__, "0x%08X %s type == %d", uid, msg, type );
        return EOK;
}

uint32_t mission_sync_ceid;

int mission_sync_init (  ) {

        uint8_t user_data [ 100 ] = "Hi!";

        mission_sync_ceid = fc_interact_register_descr ( (char*) __func__,
                                                         user_data,
                                                         mission_sync_creation_parse,
                                                         mission_sync_reset );

        fc_interact_add_handler_msg ( mission_sync_ceid, mission_sync_msg );
        fc_interact_add_handler_err ( mission_sync_ceid, mission_sync_err );
        fc_interact_add_handler_inf ( mission_sync_ceid, mission_sync_inf );
        fc_interact_add_handler_abt ( mission_sync_ceid, mission_sync_abt );

        return EOK;
}

int mission_sync_request ( fc_instance_t *fc ) {
        int rv;

        if ( ( rv = fc_command_spawn ( fc, "UAV", mission_sync_ceid, "fcont mission_sync" ) ) != EOK ) {
                err_errno ( __func__, rv, "fc_command_spawn failed" );
        }

        return rv;
}

FAdapter::FAdapter(QObject *parent) :
    QObject(parent)
{
}

bool FAdapter::finit(char* address)
{
    int rv;

    printf ( "fclient test application\n" );

//	MsgTest ();

    mission_sync_init();

    fc_init_tcp ( &fclient, address /*"192.168.1.11"*/ );
    fc_start ( &fclient );

    if ( ( rv = fc_subscribe_on_object ( &fclient, ( char * ) "SERVER" ) ) != EOK ) {
        err_report ( "%s. fc_subscribe_on_object failed: %s", __func__, strerror ( rv ) );
        return false;
    }

    if ( ( rv = fc_subscribe_on_object ( &fclient, ( char * ) "UAV" ) ) != EOK ) {
        err_report ( "%s. fc_subscribe_on_object UAV failed: %s", __func__, strerror ( rv ) );
        return false;
    }

    if ( ( ftm_handle = fc_tlm_register_handler ( &fclient, (char *) "flight_data", read_telemetry ) ) != NULL ) {
        fc_tlm_attach_param ( ftm_handle, (char *) "Roll", &ftm.roll );
        fc_tlm_attach_param ( ftm_handle, (char *) "Pitch", &ftm.pitch );
        fc_tlm_attach_param ( ftm_handle, (char *) "Heading", &ftm.head );

        fc_tlm_attach_param ( ftm_handle, (char *) "Latitude", &ftm.lat );
        fc_tlm_attach_param ( ftm_handle, (char *) "Longitude", &ftm.lon );
        fc_tlm_attach_param ( ftm_handle, (char *) "Altitude", &ftm.alt );


        fc_tlm_attach_param ( ftm_handle, (char *) "Velocity", &ftm.vel );


        fc_tlm_attach_param ( ftm_handle, (char *) "B1Voltage", &ftm.b1v );
        fc_tlm_attach_param ( ftm_handle, (char *) "B2Voltage", &ftm.b2v );
        fc_tlm_attach_param ( ftm_handle, (char *) "FotoFramesNumber", &ftm.frames );
    }
    else
    {
        return false;
    }


    return true;
}

#include <QDebug>

int FAdapter::read_telemetry(void*, int)
{
    telemetry::Navigation nav;
    nav.set_x(ftm.lon);
    nav.set_y(ftm.lat);
    nav.set_altitude(ftm.alt);
    static double test_counter = 0;
    nav.set_height(test_counter++);
    nav.set_z(555.0);

    qDebug() << "TELEMETRY !!1 " << ftm.lon << ftm.alt<< ftm.b1v<< ftm.b2v<< ftm.frames<< ftm.head;

    std::string send_str_nav, send_str_orient;
    if(nav.SerializeToString(&send_str_nav))
        m_fadapter->handler_send_telemetry("navigation", QByteArray::fromRawData(send_str_nav.data(), send_str_nav.size()));

//    telemetry::Navigation navigation;
//    navigation.set_x(ui->tableWidget->item(uavtable::X,0)->data(0).toDouble());
//    navigation.set_y(ui->tableWidget->item(uavtable::Y,0)->data(0).toDouble());
//    navigation.set_z(ui->tableWidget->item(uavtable::Z,0)->data(0).toDouble());
//    navigation.set_altitude(ui->tableWidget->item(uavtable::ALTITUDE,0)->data(0).toDouble());
//    navigation.set_height(ui->tableWidget->item(uavtable::HEIGHT,0)->data(0).toDouble());

//    telemetry::UAVState uavState;
//    uavState.set_redline(ui->tableWidget->item(uavtable::REDLINE,0)->data(0).toUInt());
//    uavState.set_removal(ui->tableWidget->item(uavtable::REMOVAL,0)->data(0).toDouble());
//    uavState.set_voltage(ui->tableWidget->item(uavtable::VOLTAGE,0)->data(0).toDouble());

    telemetry::Orientation uavOrientation;
    uavOrientation.set_displacement(0);
    uavOrientation.set_pitch(ftm.pitch);
    uavOrientation.set_roll(ftm.roll);
    uavOrientation.set_rate(ftm.head);

    if(uavOrientation.SerializeToString(&send_str_orient))
        m_fadapter->handler_send_telemetry("orientation", QByteArray::fromRawData(send_str_orient.data(), send_str_orient.size()));

//    printf ( "=========================\n" );
//    printf ( "Roll \t%.2f\n", ftm.roll );
//    printf ( "Pitch\t%.2f\n", ftm.pitch );
//    printf ( "Head \t%.2f\n", ftm.head );
//    printf ( "B1   \t%.2f\n", ftm.b1v );
//    printf ( "B2   \t%.2f\n", ftm.b2v );
//    printf ( "=========================\n\n" );


    return EOK;
}

void FAdapter::slotCommandControl(QByteArray, QString* resp_str)
{
    mission_sync_request(&fclient);
 //   *(resp_str) = command_responce;
}

